
#ifndef FILTER_H
#define FILTER_H

#include <ros/ros.h>
#include <vector>
#include <SLAM/SwitchPhase.h>

typedef struct {
	float x;
	float y;
	float theta;
} Particle;

class Filter {
public:
	Filter(int num_particles);
	~Filter();

	//void robotMoved(float delta_x, float delta_y, float delta_theta);
	void robotMoved(float dist_left, float dist_right, float wheel_base);
	void estimatedPosition(float *x, float *y, float *theta);
	void cachedEstimatedPosition(float *x, float *y, float *theta);
	void resampleParticles();
	void renderParticles();

	void switch_phase_callback(const SLAM::SwitchPhase::ConstPtr& msg);
private:
	float robot_dx_;
	float robot_dy_;
	float robot_dtheta_;

	float estimate_robot_x_;
	float estimate_robot_y_;
	float estimate_robot_theta_;

	int num_particles_;
	std::vector<Particle> particles_;
	std::vector<int> cumulative_sum_;

	ros::Publisher particle_publisher_;
	ros::Subscriber phase_subscriber_;
	ros::ServiceClient probabilities_client_;
};

#endif
